package novemberBot.navigation;


import novemberBot.FactoryFloorRobot;
import novemberBot.navigation.Odometer.Direction;
import novemberBot.sensor.LineChecker;
import novemberBot.utils.Trig;

/**monitors light sensors, computes corrections to position based on line crossings
 * and updates odometer position appropriately. Can be turned on or off 
 * 
 * @author Antoine Lennartz*/
public class GridSnapper extends Thread {

	/**Distance between the 2 light sensors*/ 
	public static double LIGHT_SENSOR_DIST;
	/**enables or disables grid snapping*/
	private boolean snapEnabled=false;
	/**true if there is a new left sensor grid detection since the last correction, false otherwise*/
	private boolean newLeft=false;
	/**true if there is a new right sensor grid detection since the last correction, false otherwise*/
	private boolean newRight=false;
	/**position of robot at last left sensor grid cross*/
	private double[] posLeft = new double[3];
	/**position of robot at last right sensor grid cross*/
	private double[] posRight = new double[3];
	/**current position of robot*/
	private double[] cPos = new double[3];
	
	private LineChecker Left;
	private LineChecker Right;
	
	private Odometer odo;
	
	/**Constructor
	 * @param start - starts or not the Thread for this Object*/
	public GridSnapper(boolean start){
		odo = FactoryFloorRobot.getInstance().getOdometer();
		Left=FactoryFloorRobot.getInstance().getLeftChecker();
		Right=FactoryFloorRobot.getInstance().getRightChecker();
		LIGHT_SENSOR_DIST=FactoryFloorRobot.getInstance().getWidth()+FactoryFloorRobot.SENSOR_DIST*2;
		if(start) this.start();
	}
	
	

	
	//mutators
	/**enables or disables grid snapping*/
	public void setEnabled(boolean snapEnabled){
		if(snapEnabled){
			if(Left==null){
				Left= FactoryFloorRobot.getInstance().getLeftChecker();
			}
			if(Right==null){
				Right=FactoryFloorRobot.getInstance().getRightChecker();
			}
			Left.setEnable(true);
			Right.setEnable(true);
		}else{
			newLeft=false;
			newRight=false;
		}
		this.snapEnabled=snapEnabled;

	}
	/**polls lineCheckers and sets booleans*/
	public void run(){
		while(true){
			if(snapEnabled){
			
				odo.getPosition(cPos);
				if(Left.onLine() && !newLeft) {
					newLeft=true;
					odo.getPosition(posLeft);
				}
				if(Right.onLine() && !newRight) {
					newRight=true;
					odo.getOldPosition(posRight);
				}
				if(newLeft && newRight){
					//
					correctPos();
				}
				
				if(newLeft && Trig.distance(posLeft, cPos)>FactoryFloorRobot.getInstance().getWidth())
					newLeft=false;
				if(newRight && Trig.distance(posRight, cPos)>FactoryFloorRobot.getInstance().getWidth())
					newRight=false;
				
		//		Debugger.audibleDebug(550, 200);
			}else Thread.yield();		
		}
	}

	/** corrects odometer position based on 2 previous lineCrross*/
	private void correctPos() {
		double [] corr = new double[]{computeXCorr(), computeYCorr(), computeThetaCorr()};
		odo.corrPosition(corr, 
				new boolean[]{true, true, true});
		newLeft=false;
		newRight=false;
		
	}
	
	/**helper for correctPos
	 * @return theta correction based on posLeft and posRight*/
	private double computeThetaCorr(){
		double thetaCorr=Math.atan(Trig.distance(posLeft, posRight)/LIGHT_SENSOR_DIST);
		if(Trig.distance(cPos, posLeft)<Trig.distance(cPos, posRight)) thetaCorr=-thetaCorr;
		thetaCorr=Trig.fixAngle2(thetaCorr*180/Math.PI);
		if(Math.abs(thetaCorr)>25) thetaCorr=0;		
		return thetaCorr;
	}
	/**helper for correctPos
	 * @return X correction based on posLeft and posRight*/
	private double computeXCorr(){
		Odometer.Direction dir = odo.getDirectionCopy();
		if(dir == Direction.X_NEG || dir == Direction.X_POS){
			return axisCorr(dir);
		}else
			return 0;
	}
	
	/**helper for correctPos
	 * @return Y correction based on posLeft and posRight*/
	private double computeYCorr(){
		Odometer.Direction dir = odo.getDirectionCopy();
		if(dir == Direction.Y_NEG || dir == Direction.Y_POS){
			return axisCorr(dir);
		}else
			return 0;
	}
	/**helper for x and y correction computations*/
	private double axisCorr(Odometer.Direction dir){
		double theta=Math.atan(Trig.distance(posLeft, posRight)/LIGHT_SENSOR_DIST);
		double corr=LIGHT_SENSOR_DIST/2.0*Math.sin(theta);
		double dist;
		if(dir==Odometer.Direction.X_NEG || dir == Odometer.Direction.X_POS)
			dist=cPos[1]%30.48;
		else
			dist=cPos[0]%30.48;
		if(dist>30.48/2) dist=30.48/2.0-dist;
		
		if(dir == Odometer.Direction.X_NEG || dir == Odometer.Direction.Y_NEG)
			corr=-corr;
		
		corr-=dist;
		if(Math.abs(corr)>10) corr=0;
		return corr;
	
	}

}
